package LBC;

import java.util.ArrayList;
import java.util.Collection;

import org.apache.commons.math.optimization.GoalType;
import org.apache.commons.math.optimization.OptimizationException;
import org.apache.commons.math.optimization.RealPointValuePair;
import org.apache.commons.math.optimization.linear.LinearConstraint;
import org.apache.commons.math.optimization.linear.LinearObjectiveFunction;
import org.apache.commons.math.optimization.linear.Relationship;
import org.apache.commons.math.optimization.linear.SimplexSolver;


public class ContactThinggy {

	float amatrix[][][];
	float bvector[][];
	float fvector[][];
	
	ContactThinggy(ArrayList<Contact> Contacts){
		this.amatrix=new float[Contacts.size()][Contacts.size()][2];
		this.bvector=new float[Contacts.size()][Contacts.size()];
	}
	
	float[] normaldot(Contact c){
	
	float result[]={0f, 0f};
	int X=0;
	int Y=1;

		result[0]=-c.b.angularVelocity*c.normal[Y];
		result[1]=c.b.angularVelocity*c.normal[X];
		
	return result;
	}
	
	float[] computeAij(Contact ci, Contact cj)
	{
		float result[]={0f, 0f};
		int X=0;
		int Y=1;
		
		//if the bodies involved in ith and jth contact are distinct then aij is zero.
		if((ci.a!=cj.a)&&(ci.b!=cj.b)&&(ci.a!=cj.b)&&(ci.b!=cj.a)) return result;
		
		// what force and torque does contact j exert on body A?
		float forceA[]={0f, 0f};
		float torqueA=0f;
		
		if(cj.a==ci.a)
		{
			//force direction of jth contact force on A
			forceA=cj.normal;
			//torque direction
			torqueA=(cj.pX-ci.a.shape.getCenterX())*cj.normal[Y]-(cj.pY-ci.a.shape.getCenterY())*cj.normal[X];
		}
		else if(cj.b==ci.a)
		{
			forceA[X]=-cj.normal[X];
			forceA[Y]=-cj.normal[Y];
			torqueA=(cj.pX-ci.a.shape.getCenterX())*cj.normal[Y]-(cj.pY-ci.a.shape.getCenterY())*cj.normal[X];
		}
		
		//what force and torque does contact j exert on body B?
		float forceB[]={0f, 0f};
		float torqueB=0f;
		
		if(cj.a==ci.b)
		{
			//force direction of jth contact force on A
			forceB=cj.normal;
			//torque direction
			torqueB=(cj.pX-ci.b.shape.getCenterX())*cj.normal[Y]-(cj.pY-ci.b.shape.getCenterY())*cj.normal[X];
			
		}
		else if(cj.b==ci.b)
		{
			forceB[X]=-cj.normal[X];
			forceB[Y]=-cj.normal[Y];
			torqueB=(cj.pX-ci.b.shape.getCenterX())*cj.normal[Y]-(cj.pY-ci.b.shape.getCenterY())*cj.normal[X];
		}
		
		//how the jth contact force affects accelerations of the contact point on body A?
		float velocityAX=forceA[X]/ci.a.mass;
		float velocityAY=forceA[Y]/ci.a.mass;
		float angularVelocityA=((1/ci.a.momentOfInertia)*torqueA)*ci.rA;
		
		//how the jth contact force affects accelerations of the contact point on body B?
		float velocityBX=forceB[X]/ci.b.mass;
		float velocityBY=forceB[Y]/ci.b.mass;
		float angularVelocityB=((1/ci.b.momentOfInertia)*torqueB)*ci.rB;
		
		result[X]=ci.normal[X]*((velocityAX+angularVelocityA)-(velocityBX+angularVelocityB));
		result[Y]=ci.normal[Y]*((velocityAY+angularVelocityA)-(velocityBY+angularVelocityB));
		return result;
	}
	
	void computeAmatrix(ArrayList<Contact> Contacts){
		for (int i=0;i<Contacts.size(); i++)
		{
			for (int j=0;j<Contacts.size(); i++)
			{
				this.amatrix[i][j]=computeAij(Contacts.get(i), Contacts.get(j));
			}
		}
	}
	
	void computeBvector(ArrayList<Contact> Contacts){
		int X=0;
		int Y=1;
		
		float[] aExtPart={0f, 0f};
		float[] aVelPart={0f, 0f};
		float[] bExtPart={0f, 0f};
		float[] bVelPart={0f, 0f};
		float[] k1={0f, 0f};
		float[] ptVelA={0f, 0f};
		float[] ptVelB={0f, 0f};
		float[] k2={0f, 0f};
		
		for (int i=0;i<Contacts.size(); i++)
		{
			Contact c=Contacts.get(i);
			
			float aTorque=c.a.epsilon*c.a.momentOfInertia;
			float bTorque=c.b.epsilon*c.a.momentOfInertia; /// need to check this, should be r (cross) F
			
			float aAngularMomentum=c.a.momentOfInertia*c.a.angularVelocity;
			float bAngularMomentum=c.b.momentOfInertia*c.b.angularVelocity;
			
			aExtPart[X]=c.a.forceX/c.a.mass - ((1/c.a.momentOfInertia*aTorque)*c.rAY);
			aExtPart[Y]=c.a.forceY/c.a.mass + ((1/c.a.momentOfInertia*aTorque)*c.rAX);
			bExtPart[X]=c.b.forceX/c.b.mass - ((1/c.b.momentOfInertia*bTorque)*c.rAY);
			bExtPart[Y]=c.b.forceY/c.b.mass + ((1/c.b.momentOfInertia*bTorque)*c.rAX);
			
			aVelPart[X]=(-c.a.angularVelocity*(-c.a.angularVelocity*c.rAY)) + ((-1/c.a.momentOfInertia*(aAngularMomentum*c.a.angularVelocity))*c.rAY);
			bVelPart[Y]=(c.b.angularVelocity*(c.b.angularVelocity*c.rBX)) + ((1/c.b.momentOfInertia*(bAngularMomentum*c.b.angularVelocity))*c.rBX);
			
			float[] ndot=normaldot(c);
			
			k1[X]=c.normal[X]*((aExtPart[X]+aVelPart[X])-(bExtPart[X]+bVelPart[X]));
			k1[Y]=c.normal[Y]*((aExtPart[Y]+aVelPart[Y])-(bExtPart[Y]+bVelPart[Y]));
			
			ptVelA[X]=c.a.velocityX-(c.a.angularVelocity*c.rAY);
			ptVelA[Y]=c.a.velocityY+(c.a.angularVelocity*c.rAX);
			ptVelB[X]=c.b.velocityX-(c.b.angularVelocity*c.rBY);
			ptVelB[Y]=c.b.velocityY+(c.b.angularVelocity*c.rBX);
			
			k2[X]=2*ndot[X]*(ptVelA[X]-ptVelB[X]);
			k2[Y]=2*ndot[Y]*(ptVelA[Y]-ptVelB[Y]);
			
			this.bvector[i][X]=k1[X]+k2[X];
			this.bvector[i][Y]=k1[Y]+k2[Y];
			
			
		}
	}
	
	void computeContactForces(ArrayList<Contact> Contacts)
	{
		int X=0;
		int Y=1;
		
		computeAmatrix(Contacts);
		computeBvector(Contacts);
		computeFvector(Contacts);
		
		/*TU TRZEBA WYWOŁAĆ QP(amatrix,bvector)!!!*/
		
		
		float[] f={0f, 0f};
		float[] normal={0f, 0f};
		Contact c;
		float forceX;
		float forceY;
		float force;
		float aTorque;
		float bTorque;
		
		for(int i=0; i<Contacts.size(); i++)
		{
			f[X]=this.fvector[i][X];
			f[Y]=this.fvector[i][Y];
			normal=Contacts.get(i).normal;
			c=Contacts.get(i);
			
			forceX=f[X]*normal[X];
			forceY=f[Y]*normal[Y];
			force=(float)(Math.sqrt((Math.pow(forceX,2))+(Math.pow(forceY,2))));
			
			aTorque=c.rA*force;
			bTorque=c.rB*force;
			
			Contacts.get(i).a.accelerationX+=(forceX/c.a.mass);
			Contacts.get(i).a.accelerationY+=(forceY/c.a.mass);
			Contacts.get(i).a.epsilon+=(aTorque/c.a.momentOfInertia);
			Contacts.get(i).b.accelerationX-=(forceX/c.b.mass);
			Contacts.get(i).b.accelerationY-=(forceY/c.b.mass);
			Contacts.get(i).b.epsilon-=(bTorque/c.b.momentOfInertia);
			
		}
		
	}
	
	public void computeFvector(ArrayList<Contact> Contacts){
		for (int i=0 ; i<Contacts.size() ; i++)
			for (int j=0 ; j<Contacts.size() ; j++ )
				for(int iterator=0 ; iterator<2 ; iterator++){
				//konwersja
				String atemp=String.valueOf(amatrix[i][j][iterator]);
				double a=Double.valueOf(atemp);
				String btemp=String.valueOf(bvector[i][iterator]);
				
				double b=Double.valueOf(btemp);
				double [] arrayOfcoefficients={a,0d};
				Collection constraints = new ArrayList();
				constraints.add(new LinearConstraint(new double[] { a }, Relationship.GEQ, -b));
				constraints.add(new LinearConstraint(new double[] { 1 }, Relationship.GEQ, 0));
				LinearObjectiveFunction function=new LinearObjectiveFunction(arrayOfcoefficients, b);
				
				RealPointValuePair solution; 
				try {
					solution = new SimplexSolver().optimize(function, constraints, GoalType.MINIMIZE, false);
				} catch (OptimizationException e) {
					
					//TODO
					//ZAPYTA� CO MA WTEDY ROBI�
					
					solution=null;
					e.printStackTrace();
				}
				
				
				double result=solution.getValue();
				this.fvector[j][iterator]=(float) result;

			}
	}
}
